#include "stm32f10x.h" // Device header
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "Delay.h"
#include "Servo.h"
#include "PWM.h"
#include "AD.h"
#include "OLED.h"
#include "PID.h"
#include "Key.h"

typedef struct Indicator {
    float ActiveValue; // 当前值
    float TargetValue; // 目标值
    float OutputMax;   // 输出限幅
    float CurrError;   // k项偏差
    float LastError;   // k-1项偏差
    float PrevError;   // k-2项偏差
    float Increment;   // 输出增量
    float Transform;   // 输出转换值
} PID_Indicator;

typedef struct Indicator_ {
    float KeyNum;                     // 按键
    float UPhotoresistor;             // 上
    float DPhotoresistor;             // 下
    float LPhotoresistor;             // 左
    float RPhotoresistor;             // 右
    float ActiveValue_Horizontal_abs; // 水平当前值的绝对值
    float ActiveValue_Vertical_abs;   // 垂直当前值的绝对值
    float ActiveValue_Horizontal;     // 水平当前值
    float ActiveValue_Vertical;       // 垂直当前值
    float TargetValue_Horizontal;     // 水平目标值
    float TargetValue_Vertical;       // 垂直目标值
    float OutputMax_Horizontal;       // 水平输出限幅
    float OutputMax_Vertical;         // 垂直输出限幅
} SYS_Indicator;

SYS_Indicator SYS_initial(void)
{
    SYS_Indicator system;
    system.UPhotoresistor             = 0;
    system.DPhotoresistor             = 0;
    system.LPhotoresistor             = 0;
    system.RPhotoresistor             = 0;
    system.ActiveValue_Horizontal_abs = 0;
    system.ActiveValue_Vertical_abs   = 0;
    system.ActiveValue_Horizontal     = 0;
    system.ActiveValue_Vertical       = 0;
    system.TargetValue_Horizontal     = 0;
    system.TargetValue_Vertical       = 0;
    system.OutputMax_Horizontal       = 200;
    system.OutputMax_Vertical         = 200;
    return system;
}

SYS_Indicator GetPhotoresistorValue(void)
{
    SYS_Indicator system;
    system.UPhotoresistor = (AD_GetValue(ADC_Channel_4)) / 10;
    system.DPhotoresistor = (AD_GetValue(ADC_Channel_7)) / 10;
    system.LPhotoresistor = (AD_GetValue(ADC_Channel_6)) / 10;
    system.RPhotoresistor = (AD_GetValue(ADC_Channel_5)) / 10;

    float ActiveValue_Horizontal = system.LPhotoresistor - system.RPhotoresistor;
    float ActiveValue_Vertical   = system.UPhotoresistor - system.DPhotoresistor;

    system.ActiveValue_Horizontal_abs = abs(ActiveValue_Horizontal);
    system.ActiveValue_Vertical_abs   = abs(ActiveValue_Vertical);

    system.ActiveValue_Horizontal = ActiveValue_Horizontal;
    system.ActiveValue_Vertical   = ActiveValue_Vertical;
    system.OutputMax_Horizontal   = 200;
    system.OutputMax_Vertical     = 200;
    system.TargetValue_Horizontal = 0;
    system.TargetValue_Vertical   = 0;
    return system;
}

void OLED_SetUp(void)
{
    OLED_ShowString(1, 1, "U");
    OLED_ShowString(2, 1, "D");
    OLED_ShowString(3, 1, "L");
    OLED_ShowString(4, 1, "R");

    OLED_ShowString(1, 8, "IV");
    OLED_ShowString(2, 8, "AV");
    OLED_ShowString(3, 8, "IH");
    OLED_ShowString(4, 8, "AH");
}

void OLED_Printf(PID_Indicator *indicator_horizontal, PID_Indicator *indicator_vertical, SYS_Indicator *system)
{
    OLED_ShowNum(1, 3, system->UPhotoresistor, 4);
    OLED_ShowNum(2, 3, system->DPhotoresistor, 4);
    OLED_ShowNum(3, 3, system->LPhotoresistor, 4);
    OLED_ShowNum(4, 3, system->RPhotoresistor, 4);

    OLED_ShowNum(1, 12, indicator_vertical->Transform * 100, 4);
    OLED_ShowSignedNum(2, 11, system->ActiveValue_Vertical, 4);
    OLED_ShowNum(3, 12, indicator_horizontal->Transform * 100, 4);
    OLED_ShowSignedNum(4, 11, system->ActiveValue_Horizontal, 4);
}

void ProgramStart(void)
{
    PID_Indicator indicator_horizontal = PID_initial(); // 算法水平结构体初始化
    PID_Indicator indicator_vertical   = PID_initial(); // 算法垂直结构体初始化
    SYS_Indicator system               = SYS_initial(); // 系统结构体初始化

    system = GetPhotoresistorValue(); // 获取光敏电阻分压值

    /*比较两个方向上的差值（绝对值）大小*/
    if (system.ActiveValue_Horizontal_abs > system.ActiveValue_Vertical_abs) {
        OLED_Printf(&indicator_horizontal, &indicator_vertical, &system);
        PID_setParameters(&indicator_horizontal, system.ActiveValue_Horizontal, system.TargetValue_Horizontal, system.OutputMax_Horizontal); // 根据获取的当前值，将相关系统结构体成员值赋给算法结构体
        do {
            PID_algorithmCall(&indicator_horizontal);        // 算法调用
            Servo2_SetAngle(indicator_horizontal.Transform); // 舵机转速控制
            Delay_ms(200);                                   // 维持PWM信号200ms
            OLED_Printf(&indicator_horizontal, &indicator_vertical, &system);
        } while (indicator_horizontal.Transform != 15.0);
    }

    else if (system.ActiveValue_Horizontal_abs < system.ActiveValue_Vertical_abs) {
        OLED_Printf(&indicator_horizontal, &indicator_vertical, &system);
        PID_setParameters(&indicator_vertical, system.ActiveValue_Vertical, system.TargetValue_Vertical, system.OutputMax_Vertical); // 根据获取的当前值，将相关系统结构体成员值赋给算法结构体
        do {
            PID_algorithmCall(&indicator_vertical);        // 算法调用
            Servo1_SetAngle(indicator_vertical.Transform); // 舵机转速控制
            Delay_ms(200);                                 // 维持PWM信号200ms
            OLED_Printf(&indicator_horizontal, &indicator_vertical, &system);
        } while (indicator_vertical.Transform != 15.0);
    }

    else
        OLED_Printf(&indicator_horizontal, &indicator_vertical, &system);
    return;
}

void ProgramStart0(void)
{
    PID_Indicator indicator_horizontal = PID_initial(); // 算法水平结构体初始化
    PID_Indicator indicator_vertical   = PID_initial(); // 算法垂直结构体初始化
    SYS_Indicator system               = SYS_initial(); // 系统结构体初始化

    goto FLAG_3;

FLAG_1:
    PID_setParameters(&indicator_horizontal, system.ActiveValue_Horizontal, system.TargetValue_Horizontal, system.OutputMax_Horizontal); // 根据获取的当前值，将相关系统结构体成员值赋给算法结构体
    do {
        PID_algorithmCall(&indicator_horizontal);        // 算法调用
        Servo2_SetAngle(indicator_horizontal.Transform); // 舵机转速控制
        Delay_ms(200);                                   // 维持PWM信号200ms
        OLED_Printf(&indicator_horizontal, &indicator_vertical, &system);
    } while (indicator_horizontal.Transform != 15.0);
    goto FLAG_3;

FLAG_2:
    PID_setParameters(&indicator_vertical, system.ActiveValue_Vertical, system.TargetValue_Vertical, system.OutputMax_Vertical); // 根据获取的当前值，将相关系统结构体成员值赋给算法结构体
    do {
        PID_algorithmCall(&indicator_vertical);        // 算法调用
        Servo1_SetAngle(indicator_vertical.Transform); // 舵机转速控制
        Delay_ms(200);                                 // 维持PWM信号200ms
        OLED_Printf(&indicator_horizontal, &indicator_vertical, &system);
    } while (indicator_vertical.Transform != 15.0);
    goto FLAG_3;

FLAG_3:

    system = GetPhotoresistorValue(); // 获取光敏电阻分压值
    // 比较两个方向上的差值（绝对值）大小
    if (system.ActiveValue_Horizontal_abs > system.ActiveValue_Vertical_abs) {
        OLED_Printf(&indicator_horizontal, &indicator_vertical, &system);
        goto FLAG_1;
    } else if (system.ActiveValue_Horizontal_abs < system.ActiveValue_Vertical_abs) {
        goto FLAG_2;
    } else
        OLED_Printf(&indicator_horizontal, &indicator_vertical, &system);
    goto FLAG_3;
}

void ProgramStop(void)
{
    PID_Indicator indicator_horizontal = PID_initial(); // 算法水平结构体初始化
    PID_Indicator indicator_vertical   = PID_initial(); // 算法垂直结构体初始化
    SYS_Indicator system               = SYS_initial(); // 系统结构体初始化

    system = GetPhotoresistorValue(); // 获取光敏电阻分压值
    OLED_Printf(&indicator_horizontal, &indicator_vertical, &system);
    Delay_ms(20);
    return;
}

int main(void)
{
    Servo_Init();
    AD_Init();
    Key_Init();
    Key_Init_();
    OLED_Init();
    OLED_SetUp();

    while (1) {

        SYS_Indicator system = SYS_initial(); // 创建系统结构体初始化

        system.KeyNum = Key_GetNum();
        if (system.KeyNum == 8) {
            ProgramStart();
        } else if (system.KeyNum == 9) {
            ProgramStop();
        }
		
		// ProgramStart0();
    }
}
